function [out, auxOut] = icastep(mode, in, cfg, par, cst) %#codegen TODO: persistent cfg & par
%ICASTEP 惯性系初始对准解算周期
%
% Tips
% # 参考坐标系L为NED
% # 当对准时间小于t1时，使用第1s数据作为双矢量定姿所用的前一组数据，当对准时间不小于t1时，使用t1时刻数据作为双矢量定姿所用的前一组数据
%
% Input Arguments
% # mode: int8标量，运行模式：0为初始化，1为对准解算，其它为输出上次的结果
% # in: 结构体，包含以下域：
%     specVelInc: 3*1列向量，比速度增量，单位m/s
%     angleInc: 3*1列向量，角增量，单位rad
%     vN: 3*1列向量，GPS输出导航系下速度，静基座对准不使用，单位m/s
%     L: 标量，本解算周期纬度，对静基座对准，在同一次对准过程中应保持不变，单位rad
%     lambda: 标量，本解算周期经度，静基座对准不使用，单位rad
%     h: 标量，本解算周期高度，静基座对准不使用，单位rad
%     g: 标量，本解算周期重力加速度值，对静基座对准，在同一次对准过程中应保持不变，单位m/s^2
%     doPolyFit: 逻辑标量，true表示对B_I0下比速度进行2阶多项式拟合，false表示直接使用原始值，可以仅在粗对准结束时刻设置为true以减少计算量，仅在vehicleIsStatic为true时有效
% # cfg: 结构体，配置量，在同一次对准过程中应保持不变，包含以下域：
%     vehicleIsStatic: 逻辑标量，true表示进行（准）静基座对准，false表示进行动基座对准
%     bufLen: int32标量，缓存长度，应不小于对准总采样数除以bufInterval
%     bufInterval: int32标量，缓存间隔
%     calcMethod: uint16标量，计算方法标志，1表示双矢量，2表示多矢量-SVD方法，3表示多矢量-QUEST递增，4表示多矢量-QUEST差分，5表示多矢量-eular_q递增
%     useSimpDlbVecOrient: 逻辑标量，true表示双矢量定姿时使用简单矢量，用于调试误差仿真脚本，否则为false
%     latEstIntLen: int32标量，纬度估计积分长度，即采样周期的倍数
% # par: 结构体，参量，在同一次对准过程中应保持不变，包含以下域：
%     samplePeriod: 标量，惯组输出的采样周期，单位s
%     t1: 标量，双矢量定姿所用的前一组数据对应的时刻点，建议设为粗对准时间长度的一半，当vehicleIsStatic为true时，该参量实际上为输入in，单位s
%     vecInterval: int32标量，多矢量定姿矢量间隔
% # cst: 结构体，常量，在同一次对准过程中应保持不变，包含以下域：
%     omegaIE: 标量，地球自转角速率，单位rad/s
%     RE: 标量，地球长半轴，单位m
%     eSq: 标量，地球偏心率的平方，无单位
%
% Output Arguments
% # out: 结构体，输出量，包含以下域：
%     cycCnt: int32标量，解算周期计数
%     CBL: 3*3矩阵，方向余弦矩阵
%     CB0L: 3*3矩阵，0时刻方向余弦矩阵
% # auxOut: 结构体，辅助输出量，包含以下域：
%     CB_I0E_I0: 3*3矩阵
%     CBB_I0: 3*3矩阵
%     vSFB_I0: 3*1向量
%     vSFB_I0Fit: 3*1向量
%     vSFB_I0Fit_t1: 3*1向量
%     vSFE_I0: 3*1向量
%     L: 标量，估算纬度
%
% References:
% # 《应用导航技术》“惯性系粗对准算法”一节（本函数中的E系相当于文档中的E'系）
% # 一种改进的晃动基座上纬度自估计方法
% # 《应用导航算法工程基础》“准静基座初始对准”

coder.extrinsic('warning');

persistent p_ICASS_mid p_ICASS_out p_ICASS_auxOut

% persistent变量初始化
if isempty(p_ICASS_mid) || isempty(p_ICASS_out)
    p_ICASS_mid.CBB_I0_1 = coder.nullcopy(NaN(3));
    p_ICASS_mid.Dalpha_lx = coder.nullcopy(NaN(3, 2));
    p_ICASS_mid.Dupsilon_lx = coder.nullcopy(NaN(3, 2));
    p_ICASS_mid.vSFB_I0_1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.vSFE_I0_1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.vSFB_I0_t1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.vSFE_I0_t1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.vSFB_I0Buf = coder.nullcopy(NaN(3, cfg.bufLen));
    p_ICASS_mid.DvSFB_I0Buf = coder.nullcopy(NaN(3, cfg.latEstIntLen));
    p_ICASS_mid.DvSFB_I0_t1_sum = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.omegaIBB_I0Int_1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.t1CycCnt = coder.nullcopy(int32(0));
    p_ICASS_mid.tBuf = coder.nullcopy(NaN(1, cfg.bufLen));
    p_ICASS_mid.bufInterval = coder.nullcopy(int32(-1));
    p_ICASS_mid.bufCnt = coder.nullcopy(int32(0));
    p_ICASS_mid.lambda0 = coder.nullcopy(NaN);
    p_ICASS_mid.t = coder.nullcopy(NaN);
    p_ICASS_mid.CNE_I0_1 = coder.nullcopy(NaN(3));
    p_ICASS_mid.vN_1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_mid.omega_1 = coder.nullcopy(NaN(3, 1)); % omegaINN+omegaIEN
    p_ICASS_mid.B = coder.nullcopy(NaN(3));
    p_ICASS_mid.vNum = coder.nullcopy(int32(0));
    p_ICASS_mid.V = coder.nullcopy(NaN(3, cfg.bufLen));
    p_ICASS_mid.W = coder.nullcopy(NaN(3, cfg.bufLen));
    p_ICASS_mid.CE_I0N_0 = coder.nullcopy(NaN(3));
    
    p_ICASS_out.cycCnt = coder.nullcopy(int32(1));
    p_ICASS_out.CBL = coder.nullcopy(NaN(3));
    p_ICASS_out.CB0L = coder.nullcopy(NaN(3));
    
    p_ICASS_auxOut.CB_I0E_I0 = coder.nullcopy(NaN(3));
    p_ICASS_auxOut.CBB_I0 = coder.nullcopy(NaN(3));
    p_ICASS_auxOut.vSFB_I0 = coder.nullcopy(NaN(3, 1));
    p_ICASS_auxOut.vSFB_I0Fit = coder.nullcopy(NaN(3, 1));
    p_ICASS_auxOut.vSFB_I0Fit_t1 = coder.nullcopy(NaN(3, 1));
    p_ICASS_auxOut.vSFE_I0 = coder.nullcopy(NaN(3, 1));
    p_ICASS_auxOut.L = coder.nullcopy(NaN);
end
switch mode
    case int8(0)
        % 零时刻初始化
        p_ICASS_mid.CBB_I0_1 = eye(3);
        p_ICASS_mid.Dalpha_lx = zeros(3, 2);
        p_ICASS_mid.Dupsilon_lx = zeros(3, 2);
        p_ICASS_mid.vSFB_I0_1 = zeros(3, 1);
        p_ICASS_mid.vSFB_I0_t1 = NaN(3, 1);
        p_ICASS_mid.vSFE_I0_t1 = NaN(3, 1);
        p_ICASS_mid.vSFB_I0Buf = NaN(3, cfg.bufLen);
        p_ICASS_mid.DvSFB_I0Buf = zeros(3, cfg.latEstIntLen);
        p_ICASS_mid.DvSFB_I0_t1_sum = NaN(3, 1);
        p_ICASS_mid.omegaIBB_I0Int_1 = zeros(3, 1);
        p_ICASS_mid.t1CycCnt = int32(-1);
        p_ICASS_mid.tBuf = NaN(1, cfg.bufLen);
        p_ICASS_mid.bufCnt = int32(0);
        p_ICASS_mid.lambda0 = in.lambda; % 0时刻GPS输出经度
        p_ICASS_mid.t = 0;
        if cfg.vehicleIsStatic
            p_ICASS_mid.vSFE_I0_1 = NaN(3, 1);
            p_ICASS_mid.CNE_I0_1 = NaN(3);
            p_ICASS_mid.vN_1 = NaN(3, 1);
            p_ICASS_mid.omega_1 = NaN(3, 1);
        else
            p_ICASS_mid.vSFE_I0_1 = zeros(3, 1);
            p_ICASS_mid.CNE_I0_1 = dcmile2enu(p_ICASS_mid.t, in.L, cst.omegaIE, 0)'; % REF1式22
            p_ICASS_mid.vN_1 = in.vN; % 0时刻GPS输出导航系下速度
            p_ICASS_mid.omega_1 = calcomega(in.L, in.h, cst.RE, cst.eSq, cst.omegaIE, in.vN); % 0时刻角速率
        end
        p_ICASS_mid.B = zeros(3);
        p_ICASS_mid.vNum = int32(0);
        p_ICASS_mid.V = NaN(3, cfg.bufLen);
        p_ICASS_mid.W = NaN(3, cfg.bufLen);
        p_ICASS_mid.bufInterval = cfg.bufInterval;
        p_ICASS_mid.CE_I0N_0 = [0 1 0; -sin(in.L) 0 cos(in.L); cos(in.L) 0 sin(in.L)];
        
        p_ICASS_out.cycCnt = int32(0);
        p_ICASS_out.CBL = NaN(3);
        
        p_ICASS_auxOut.CB_I0E_I0 = NaN(3);
        p_ICASS_auxOut.CBB_I0 = eye(3);
        p_ICASS_auxOut.vSFB_I0 = zeros(3, 1);
        p_ICASS_auxOut.vSFB_I0Fit = NaN(3, 1);
        p_ICASS_auxOut.vSFB_I0Fit_t1 = NaN(3, 1);
        p_ICASS_auxOut.vSFE_I0 = zeros(3, 1);
        p_ICASS_auxOut.L = NaN;
    case int8(1)
        p_ICASS_mid.t = p_ICASS_mid.t + par.samplePeriod;
        p_ICASS_out.cycCnt = p_ICASS_out.cycCnt + int32(1);
        % 读入器件输出
        if p_ICASS_out.cycCnt == 1
            p_ICASS_mid.Dupsilon_lx = horzcat(in.specVelInc, in.specVelInc);
            p_ICASS_mid.Dalpha_lx = horzcat(in.angleInc, in.angleInc);
        else
            p_ICASS_mid.Dupsilon_lx = horzcat(in.specVelInc, p_ICASS_mid.Dupsilon_lx(:, 1:(end-1)));
            p_ICASS_mid.Dalpha_lx = horzcat(in.angleInc, p_ICASS_mid.Dalpha_lx(:, 1:(end-1)));
        end
        
        % 解算姿态-方向余弦矩阵与比力积分
        bodycoordattupdstep(0);
        bodycoordspecvelupdstep(0);
        [~, ~, alpha_l, alpha_l1, beta_l] = bodycoordattupdstep(1, p_ICASS_mid.Dalpha_lx);
        bodycoordspecvelupdstep(1, p_ICASS_mid.Dupsilon_lx, alpha_l1, p_ICASS_mid.Dalpha_lx);
        [DvSFB_m1_m] = bodycoordspecvelupdstep(2, p_ICASS_mid.Dupsilon_lx, alpha_l1, p_ICASS_mid.Dalpha_lx, alpha_l);
        [CB_mB_m1, ~] = bodycoordattupdstep(2, p_ICASS_mid.Dalpha_lx, true);
        
        % 计算B系相对于B_I0系姿态及B_I0下比速度
        CBB_I0 = p_ICASS_mid.CBB_I0_1 * CB_mB_m1;
        DvSFB_I0 = p_ICASS_mid.CBB_I0_1 * DvSFB_m1_m;
        vSFB_I0 = p_ICASS_mid.vSFB_I0_1 + DvSFB_I0;
        omegaIBB_I0Int = p_ICASS_mid.omegaIBB_I0Int_1 + p_ICASS_mid.CBB_I0_1 * (alpha_l+2*beta_l);
        
        CE_I0N = dcmile2enu(p_ICASS_mid.t, in.L, cst.omegaIE, in.lambda-p_ICASS_mid.lambda0); % REF1式22
        
        if cfg.vehicleIsStatic
            if (mod(p_ICASS_out.cycCnt, p_ICASS_mid.bufInterval)==0) % 在静基座条件下保存历史vSFB_I0值以用于后续拟合
                p_ICASS_mid.bufCnt = p_ICASS_mid.bufCnt + 1;
                if p_ICASS_mid.bufCnt == cfg.bufLen
                    p_ICASS_mid.vSFB_I0Buf(:, p_ICASS_mid.bufCnt) = vSFB_I0;
                    p_ICASS_mid.tBuf(:, p_ICASS_mid.bufCnt) = p_ICASS_mid.t;
                    
                    tmp_vSFB_I0Buf = p_ICASS_mid.vSFB_I0Buf(:, 1:2:end); % 间隔增加一倍
                    p_ICASS_mid.bufCnt = int32(size(tmp_vSFB_I0Buf, 2));
                    p_ICASS_mid.vSFB_I0Buf = NaN(size(p_ICASS_mid.vSFB_I0Buf));
                    p_ICASS_mid.vSFB_I0Buf(:, 1:p_ICASS_mid.bufCnt) = tmp_vSFB_I0Buf;
                    tmp_tBuf = p_ICASS_mid.tBuf(:, 1:2:end); % 间隔增加一倍
                    p_ICASS_mid.tBuf = NaN(size(p_ICASS_mid.tBuf));
                    p_ICASS_mid.tBuf(:, 1:p_ICASS_mid.bufCnt) = tmp_tBuf;
                    
                    p_ICASS_mid.bufInterval = p_ICASS_mid.bufInterval*2;
                else
                    p_ICASS_mid.vSFB_I0Buf(:, p_ICASS_mid.bufCnt) = vSFB_I0;
                    p_ICASS_mid.tBuf(:, p_ICASS_mid.bufCnt) = p_ICASS_mid.t;
                end
            end
            % 计算E_I0下比速度
            vSFE_I0 = [cos(in.L)*sin(cst.omegaIE*p_ICASS_mid.t)/cst.omegaIE ...
                cos(in.L)*(1-cos(cst.omegaIE*p_ICASS_mid.t))/cst.omegaIE ...
                sin(in.L)*p_ICASS_mid.t]' * in.g; % REF1式39
            p_ICASS_mid.vSFE_I0_t1 = [cos(in.L)*sin(cst.omegaIE*par.t1)/cst.omegaIE ...
                cos(in.L)*(1-cos(cst.omegaIE*par.t1))/cst.omegaIE ...
                sin(in.L)*par.t1]' * in.g;
            omegaIEE_I0Int = [0 0 cst.omegaIE*p_ICASS_mid.t]';
            
            % 最小二乘拟合以平滑B_I0下当前时刻和t1时刻比速度
            if in.doPolyFit % TODO: （准）静基座对准均采用拟合方式，可以设置拟合周期
                vSFB_I0Fit = NaN(3, 1);
                vSFB_I0Fit_t1 = NaN(3, 1);
                for i=1:3
                    % 当前时刻
                    p = polyfit(p_ICASS_mid.tBuf(1:p_ICASS_mid.bufCnt), p_ICASS_mid.vSFB_I0Buf(i, 1:p_ICASS_mid.bufCnt), 3);
                    p(end) = 0;
                    vSFB_I0Fit(i, 1) = polyval(p, p_ICASS_mid.t);
                    % t1时刻
                    vSFB_I0Fit_t1(i, 1) = polyval(p, par.t1);
                end
                vSFB_I0_effective = vSFB_I0Fit;
                vSFB_I0_t1_effective = vSFB_I0Fit_t1;
                p_ICASS_auxOut.vSFB_I0Fit = vSFB_I0Fit;
                p_ICASS_auxOut.vSFB_I0Fit_t1 = vSFB_I0Fit_t1;
            else
                vSFB_I0_effective = vSFB_I0;
                vSFB_I0_t1_effective = p_ICASS_mid.vSFB_I0_t1;
            end
        else
            % 计算E_I0下比速度
            omega = calcomega(in.L, in.h, cst.RE, cst.eSq, cst.omegaIE, in.vN);
            vSFE_I0 = p_ICASS_mid.vSFE_I0_1 + calcDvSFE_I0(CE_I0N', p_ICASS_mid.CNE_I0_1, in.vN, p_ICASS_mid.vN_1, omega, p_ICASS_mid.omega_1, par.samplePeriod, in.g);
            p_ICASS_mid.vSFE_I0_1 = vSFE_I0;
            p_ICASS_mid.CNE_I0_1 = CE_I0N';
            p_ICASS_mid.vN_1 = in.vN;
            p_ICASS_mid.omega_1 = omega;
            % 记录t1时刻的vSFB_I0和vSFE_I0
            if p_ICASS_out.cycCnt == int32(round(par.t1/par.samplePeriod)) ...
                    || (all(isnan(p_ICASS_mid.vSFB_I0_t1)) && (p_ICASS_out.cycCnt==int32(round(1/par.samplePeriod)))) % 当没有到t1时刻时，将第1s作为t1时刻
                p_ICASS_mid.vSFB_I0_t1 = vSFB_I0;
                p_ICASS_mid.vSFE_I0_t1 = vSFE_I0;
                p_ICASS_mid.t1CycCnt = p_ICASS_out.cycCnt;
            end
            vSFB_I0_effective = vSFB_I0;
            vSFB_I0_t1_effective = p_ICASS_mid.vSFB_I0_t1;
        end
        
        % 估算纬度
        p_ICASS_mid.DvSFB_I0Buf(:, mod(p_ICASS_out.cycCnt-1, cfg.latEstIntLen)+1) = DvSFB_I0;
        if p_ICASS_out.cycCnt == cfg.latEstIntLen
            p_ICASS_mid.DvSFB_I0_t1_sum = vSFB_I0;
        elseif p_ICASS_out.cycCnt > cfg.latEstIntLen
            alpha = cst.omegaIE*(p_ICASS_mid.t-par.samplePeriod*double(cfg.latEstIntLen));
            DvSFB_I0_sum = sum(p_ICASS_mid.DvSFB_I0Buf, 2);
            theta = acos((p_ICASS_mid.DvSFB_I0_t1_sum'*DvSFB_I0_sum)/sqrt((p_ICASS_mid.DvSFB_I0_t1_sum'*p_ICASS_mid.DvSFB_I0_t1_sum)*(DvSFB_I0_sum'*DvSFB_I0_sum)));
            x = (sin(theta/2)/sin(alpha/2));
            if x < 1 && x > -1
                p_ICASS_auxOut.L = acos(x);
            end
        end
        
        switch cfg.calcMethod
            case  uint16(1)
                % 双矢量定姿
                if (p_ICASS_mid.t > par.t1) && (~cfg.vehicleIsStatic || (cfg.vehicleIsStatic && in.doPolyFit)) % 两者相等时t1=t2，逆矩阵奇异，静态时仅当拟合的时候进入
                    if cfg.useSimpDlbVecOrient
                        CB_I0E_I0 = ([vSFB_I0_t1_effective vSFB_I0_effective cross(vSFB_I0_t1_effective, vSFB_I0_effective)]...
                            / [p_ICASS_mid.vSFE_I0_t1 vSFE_I0 cross(p_ICASS_mid.vSFE_I0_t1, vSFE_I0)])'; % NOTE: 简化式
                    else
                        CB_I0E_I0 = ([vSFB_I0_t1_effective cross(vSFB_I0_t1_effective, vSFB_I0_effective) cross(vSFB_I0_t1_effective, cross(vSFB_I0_t1_effective, vSFB_I0_effective))]...
                            / [p_ICASS_mid.vSFE_I0_t1 cross(p_ICASS_mid.vSFE_I0_t1, vSFE_I0) cross(p_ICASS_mid.vSFE_I0_t1, cross(p_ICASS_mid.vSFE_I0_t1, vSFE_I0))])'; % REF1式31
                    end
                    CB_I0E_I0 = dcmnormortho_order(CB_I0E_I0); % TODO: 考核pgs与optimized规范化效果
                    p_ICASS_auxOut.CB_I0E_I0 = CB_I0E_I0;
                else
                    p_ICASS_auxOut.CB_I0E_I0 = p_ICASS_auxOut.CB_I0E_I0; %　输出之前的值，认为短时间不变
                end
            case uint16(2)
                % 多矢量定姿 % SVD方法
                if (mod(p_ICASS_out.cycCnt, par.vecInterval)==0) && ~any(isnan(vSFB_I0_effective))
                    p_ICASS_mid.B = p_ICASS_mid.B + vSFE_I0 * vSFB_I0_effective';
                    [U, ~, V] = svd(p_ICASS_mid.B);
                    CB_I0E_I0 = U*diag([1 1 det(U)*det(V)])*V';
                    CB_I0E_I0 = dcmnormortho_optimized(CB_I0E_I0);
                    p_ICASS_auxOut.CB_I0E_I0 = CB_I0E_I0;
                end
            case uint16(3)
                % 多矢量定姿 % QUEST 递增方法
                if (mod(p_ICASS_out.cycCnt, par.vecInterval)==0) && ~any(isnan(vSFB_I0_effective))
                    p_ICASS_mid.vNum = p_ICASS_mid.vNum + 1;
                    p_ICASS_mid.V(:, p_ICASS_mid.vNum) = vSFE_I0;
                    p_ICASS_mid.W(:, p_ICASS_mid.vNum) = vSFB_I0_effective;
                end
                if mod(p_ICASS_out.cycCnt, par.vecInterval) == 0 && p_ICASS_mid.vNum >= 2
                    CB_I0E_I0 = multvecattdet_quest(p_ICASS_mid.V(:, 1:p_ICASS_mid.vNum), p_ICASS_mid.W(:, 1:p_ICASS_mid.vNum));
                    p_ICASS_auxOut.CB_I0E_I0 = CB_I0E_I0;
                end
            case uint16(4)
                % 多矢量定姿 % QUEST 差分方法
                if (mod(p_ICASS_out.cycCnt, par.vecInterval)==0) && ~any(isnan(vSFB_I0_effective))
                    p_ICASS_mid.vNum = p_ICASS_mid.vNum + 1;
                    p_ICASS_mid.V(:, p_ICASS_mid.vNum) = vSFE_I0;
                    p_ICASS_mid.W(:, p_ICASS_mid.vNum) = vSFB_I0_effective;
                end
                if mod(p_ICASS_out.cycCnt, par.vecInterval) == 0 && p_ICASS_mid.vNum >= 2
                    CB_I0E_I0 = multvecattdet_quest(diff(p_ICASS_mid.V(:, 1:p_ICASS_mid.vNum), 1, 2), diff(p_ICASS_mid.W(:, 1:p_ICASS_mid.vNum), 1, 2));
                    p_ICASS_auxOut.CB_I0E_I0 = CB_I0E_I0;
                end
            case uint16(5)
                % 多矢量定姿 % eular_q 递增方法
                if (mod(p_ICASS_out.cycCnt, par.vecInterval)==0) && ~any(isnan(vSFB_I0_effective))
                    p_ICASS_mid.vNum = p_ICASS_mid.vNum + 1;
                    p_ICASS_mid.V(:, p_ICASS_mid.vNum) = vSFB_I0_effective;
                    p_ICASS_mid.W(:, p_ICASS_mid.vNum) = vSFE_I0;
                end
                if mod(p_ICASS_out.cycCnt, par.vecInterval) == 0 && p_ICASS_mid.vNum >= 2
                    CB_I0E_I0 = multvecattdet_davq(p_ICASS_mid.V(:, 1:p_ICASS_mid.vNum), p_ICASS_mid.W(:, 1:p_ICASS_mid.vNum))';
                    CB_I0E_I0 = dcmnormortho_optimized(CB_I0E_I0);
                    p_ICASS_auxOut.CB_I0E_I0 = CB_I0E_I0;
                end
            otherwise
                warning('Unexpected Method.');
        end
        CBE_I0 = p_ICASS_auxOut.CB_I0E_I0 * CBB_I0; % REF1式23
        CBN = CE_I0N * CBE_I0; % REF1式17
        p_ICASS_out.CB0L = [0 1 0; 1 0 0; 0 0 -1]' * p_ICASS_mid.CE_I0N_0 * p_ICASS_auxOut.CB_I0E_I0;
        p_ICASS_out.CBL = [0 1 0; 1 0 0; 0 0 -1]' * CBN;
        p_ICASS_mid.CBB_I0_1 = CBB_I0;
        p_ICASS_mid.vSFB_I0_1 = vSFB_I0;
        p_ICASS_mid.omegaIBB_I0Int_1 = omegaIBB_I0Int;
        p_ICASS_auxOut.CBB_I0 = CBB_I0;
        p_ICASS_auxOut.vSFB_I0 = vSFB_I0;
        p_ICASS_auxOut.vSFE_I0 = vSFE_I0;
    otherwise
        % 仅输出，不进行任何操作
end
out = p_ICASS_out;
auxOut = p_ICASS_auxOut;
end

function [DvSFE_I0] = calcDvSFE_I0(CNE_I0, CNE_I0_1, vN, vN_1, omega, omega_1, samplePeriod, g)
CNE_I0_half = middcm(CNE_I0_1, CNE_I0);
DvSFE_I0 = CNE_I0_half * ((vN-vN_1) + cross((omega+omega_1)/2, (vN+vN_1)/2)*samplePeriod - [0 0 -g]'*samplePeriod);
end

function [omega] = calcomega(L, h, RE, eSq, omegaIE, vN)
omega = transrate_geographic(vN, L, h, RE, eSq) + 2*[0 omegaIE*cos(L) omegaIE*sin(L)]';
end